/**
 * @file AStar.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief A*寻路类
 */

#include "AStar.h"
#include <cmath>
#include "Map.h"
#include "Params.h"
#include <cstring>
#include "utils.h"

/**
 * @brief 移动一步
 * @param dir 移动方向
*/
void AStarNode::moveStep(int dir) {
    switch(dir) {
        case Direction::RIGHT:
            this->x += 1;
            this->g += PARAMS.LINE_COST;
            break;
        case Direction::UP_RIGHT:
            this->y += 1;
            this->x += 1;
            this->g += PARAMS.INCLINE_COST;
            break;
        case Direction::UP:
            this->y += 1;
            this->g += PARAMS.LINE_COST;
            break;
        case Direction::UP_LEFT:
            this->y += 1;
            this->x -= 1;
            this->g += PARAMS.INCLINE_COST;
            break;
        case Direction::LEFT:
            this->x -= 1;
            this->g += PARAMS.LINE_COST;
            break;
        case Direction::DOWN_LEFT:
            this->y -= 1;
            this->x -= 1;
            this->g += PARAMS.INCLINE_COST;
            break;
        case Direction::DOWN:
            this->y -= 1;
            this->g += PARAMS.LINE_COST;
            break;
        case Direction::DOWN_RIGHT:
            this->y -= 1;
            this->x += 1;
            this->g += PARAMS.INCLINE_COST;
            break;
        default:
            break;
    }
}

/**
 * @brief 创建A*结点
 * @param x 横坐标
 * @param y 纵坐标
 * @param g 已有代价
 * @param parentDir 父节点移动方向
 * @param dir 移动方向
 * @param endX 终点横坐标
 * @param endY 终点纵坐标
 * @param isEmpty 是否使用空载地图
 */
AStarNode::AStarNode(int x, int y, double g, int parentDir, int dir, int endX, int endY, bool isEmpty) : x(x), y(y), g(g), dir(dir) {
    this->moveStep(this->dir);
    this->g += Direction::GetDirectionDiff(this->dir, parentDir) * PARAMS.ROTATE_COST;
    this->h = std::abs(this->x - endX) * PARAMS.LINE_COST + std::abs(this->y - endY) * PARAMS.LINE_COST;
}

/**
 * @brief 获得从起点到终点的指令序列
 * @param startX 起点横坐标
 * @param startY 起点纵坐标
 * @param endX 终点横坐标
 * @param endY 终点纵坐标
 * @param isEmpty 是否为空载
 * @param endRadius 终点半径
 * @return std::list<Cmd> 指令序列
 */
std::list<Cmd> AStar::getCmdSeq(int startX, int startY, int endX, int endY, bool isEmpty, int endRadius) {
    bool visited[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND];   // 节点是否已访问
    std::memset(visited, false, sizeof visited);

    visited[startX][startY] = true;

    std::list<AStarNode *> openSet; // 开集
    std::list<AStarNode *> closeSet; // 闭集

    AStarNode *root = new AStarNode(startX, startY, 0, Direction::NONE, Direction::NONE, endX, endY, isEmpty); // 路径树
    AStarNode *current = root; // 当前结点
    AStarNode *child = nullptr; // 孩子结点

    std::list<AStarNode *>::iterator minCostNode = openSet.begin(); // 最小代价结点
    bool isComplete = false; // 已完成寻路
    while(true) {
        // 将当前结点周围能走的点加入开集
        for(int dir = 1; dir <= 8; ++dir) { // 0方向为原地不动
            child = new AStarNode(current->x, current->y, current->g, current->dir, dir, endX, endY, isEmpty);
            if(Map::isInMap(child->x, child->y) && !Map::isObstacle(child->x, child->y, isEmpty) && !visited[child->x][child->y]) {
                // 入树
                current->childs.push_back(child);
                child->parent = current;
                // 加入开集
                openSet.push_back(child);
                // 标记走过
                visited[child->x][child->y] = true;
            } else {
                delete child;
                child = nullptr;
            }
        }

        // 开集为空，说明寻路失败
        if(openSet.empty()) {
            break;
        }

        // 将当前结点加入闭集
        closeSet.push_back(current);

        // 取出开集中最小代价点
        if(!openSet.empty()) {
            minCostNode = openSet.begin();
            for(std::list<AStarNode *>::iterator it = openSet.begin(); it != openSet.end(); ++it){
                if((*it)->cost() < (*minCostNode)->cost()) {
                    minCostNode = it;
                }
            }
            current = *minCostNode;
            openSet.erase(minCostNode);
        }

        // 判断寻路是否结束
        if((endRadius == 0 && (current->x == endX && current->y == endY)) || (endRadius > 0 && utils::isInRectRange(current->x, current->y, endX, endY, endRadius))) {
            isComplete = true;
            break;
        }
    }

    std::list<Cmd> cmdSeq; // 指令序列
    // 创建指令并压入指令序列
    if(isComplete) {
        std::list<Cmd> tmpSeq; // 临时指令序列
        while(current){
            tmpSeq.push_front(Cmd::createRunTo(current->x, current->y, current->dir, isEmpty));
            current = current->parent;
        }
        cmdSeq.splice(cmdSeq.end(), tmpSeq);
    }

    // 清空开集
    while(!openSet.empty()) {
        delete openSet.back();
        openSet.pop_back();
    }
    // 清空闭集
    while(!closeSet.empty()) {
        delete closeSet.back();
        closeSet.pop_back();
    }

    return cmdSeq;
}
